#include <Servo.h>
#include <Wire.h>

#define _ROBOT_VERSION_ "0.6"
#define _ROBOT_NAME_ "Spidy"

/****************************/
/* servo parts              */
/****************************/
#define I_LEFT_FRONT_COXA 0
#define I_LEFT_FRONT_FEMUR 1
#define I_LEFT_FRONT_TIBIA 2
#define I_LEFT_MIDDLE_COXA 3
#define I_LEFT_MIDDLE_FEMUR 4
#define I_LEFT_MIDDLE_TIBIA 5
#define I_LEFT_BACK_COXA 6
#define I_LEFT_BACK_FEMUR 7
#define I_LEFT_BACK_TIBIA 8
#define I_RIGHT_FRONT_COXA 9
#define I_RIGHT_FRONT_FEMUR 10
#define I_RIGHT_FRONT_TIBIA 11
#define I_RIGHT_MIDDLE_COXA 12
#define I_RIGHT_MIDDLE_FEMUR 13
#define I_RIGHT_MIDDLE_TIBIA 14
#define I_RIGHT_BACK_COXA 15
#define I_RIGHT_BACK_FEMUR 16
#define I_RIGHT_BACK_TIBIA 17

// define 
#define BT_BAUD 57600
#define RB_BAUD 9600

#define SENSOR_IR_FRONT 4
#define RELAY_SERVOCOMMANDS 6

#define N_SERVOS 18
Servo servos[N_SERVOS];
const int servo_ind[N_SERVOS] = {
  I_LEFT_FRONT_COXA, I_LEFT_FRONT_FEMUR, I_LEFT_FRONT_TIBIA,
  I_LEFT_MIDDLE_COXA, I_LEFT_MIDDLE_FEMUR, I_LEFT_MIDDLE_TIBIA,
  I_LEFT_BACK_COXA, I_LEFT_BACK_FEMUR, I_LEFT_BACK_TIBIA,

  I_RIGHT_FRONT_COXA, I_RIGHT_FRONT_FEMUR, I_RIGHT_FRONT_TIBIA,
  I_RIGHT_MIDDLE_COXA, I_RIGHT_MIDDLE_FEMUR, I_RIGHT_MIDDLE_TIBIA,
  I_RIGHT_BACK_COXA, I_RIGHT_BACK_FEMUR, I_RIGHT_BACK_TIBIA,
};
const int servo_pin[N_SERVOS] = {
  35, 36, 37,
  27, 28, 29,
  24, 25, 26,

  38, 39, 40,
  46, 47, 48,
  49, 50, 51
};
const int servo_cal[3][N_SERVOS] = {
  { // low 0°
    0,  0,  0,
    0,  0,  0,
    0,  0,  0,

    0,  0,  0,
    0,  0,  0,
    0,  0,  0
  }
  ,
  { // medium 90°
    25,  0,  35,
    65,  -45,  45,
    45,  10,  35,

    50,  55,  -25,
    -20,  55,  -25,
    -150,  25,  0
  }
  ,
  { // high 180°
    0,  0,  0,
    0,  0,  0,
    0,  0,  0,

    0,  0,  0,
    0,  0,  0,
    0,  0,  0
  }
};
int servo_pos[N_SERVOS] = {
  deg2ms(90), deg2ms(0), deg2ms(0),
  deg2ms(90), deg2ms(0), deg2ms(0),
  deg2ms(90), deg2ms(0), deg2ms(0),

  deg2ms(90), deg2ms(180), deg2ms(180),
  deg2ms(90), deg2ms(180), deg2ms(180),
  deg2ms(90), deg2ms(180), deg2ms(180),
};
boolean servo_update[N_SERVOS] = {
  true,  true,  true,
  true,  true,  true,
  true,  true,  true,

  true,  true,  true,
  true,  true,  true,
  true,  true,  true,
};

#define LED_MATRIX_I2C 0x06

#define LED_ANIM_BLANK 0
#define LED_ANIM_ERROR 1
#define LED_ANIM_WARNING 2
#define LED_ANIM_CYLON 3
#define LED_ANIM_GREENWHEEL 4
#define LED_ANIM_ECHO 5

void setLEDAnimation(int anim)
{
  anim = anim%6;
  
  if(Serial)
  {
    Serial.print("Set LED Animation '");
    if(anim == LED_ANIM_BLANK)
      Serial.print("blank");
    else if(anim == LED_ANIM_ERROR)
      Serial.print("error");
    else if(anim == LED_ANIM_WARNING)
      Serial.print("warning");
    else if(anim == LED_ANIM_CYLON)
      Serial.print("cylon");
    else if(anim == LED_ANIM_GREENWHEEL)
      Serial.print("green wheel");
    else if(anim == LED_ANIM_ECHO)
      Serial.print("echo");
    Serial.println("'");
  }

  char aCmd[12];
  String sCmd;
  sCmd = "#as+";
  sCmd = sCmd + anim;
  sCmd = sCmd + "$";
  sCmd.toCharArray(aCmd, sizeof(aCmd));
  Wire.beginTransmission(LED_MATRIX_I2C);
  Wire.write(aCmd);
  Wire.endTransmission();
}

int set_relay_servos(boolean bActive)
{
  digitalWrite(RELAY_SERVOCOMMANDS, bActive?HIGH:LOW);
  return 1;
}

boolean g_isReady = false;
boolean g_isConnected = false;
unsigned long g_time=0;
boolean g_isDebug = false;
void setup()
{
  pinMode(RELAY_SERVOCOMMANDS, OUTPUT);
  
  if(Serial)
  {
    Serial.begin(57600); // standard output ^^

    Serial.print("Robot '");
    Serial.print(_ROBOT_NAME_);
    Serial.println("' by NooTe");
    Serial.print("version ");
    Serial.println(_ROBOT_VERSION_);
    Serial.println("");
  }
  if(Serial1)
  {
    if(Serial)
    {
      Serial.print("Initialize bluetooth baud as ");
      Serial.println(BT_BAUD);
    }

    Serial1.begin(BT_BAUD); // BT serial
  }
  else
  {
    if(Serial)
      Serial.println("No bluetooth detected on Serial1");
  }
  Wire.begin(); // join i2c bus (address optional for master)
  setLEDAnimation(LED_ANIM_GREENWHEEL);

  if(Serial)
    Serial.println("Initialize servomotors...");

  // turn on relay for servocommands :)
  delay(500);
  set_relay_servos(true);
  delay(150);
  
  init_servos();
  set_pack_position();
  update_servos();
  delay(2500);
  set_relay_servos(false);
  
  if(Serial)
    Serial.println("Wait commands...");
  g_isReady = true;

  setLEDAnimation(LED_ANIM_CYLON);
  g_time = millis();
}

#define TICK_MESURE (1000/60)
#define RANGE_MESURE (1000/5)
void loop()
{
  static unsigned long tick_release = 0;    
  static unsigned long range_release = 0;

  unsigned long cur_time = millis();
  unsigned long slicetime = cur_time-g_time;
  g_time = cur_time;

  // got bluetooth command ?
  check_commands();

  // update servo positions
  update_servos();

  // send tick
  if(g_isConnected)
  {
    tick_release += slicetime;
    if(tick_release>TICK_MESURE)
    {
      send_tick(tick_release);
      tick_release-=TICK_MESURE;
    }

    range_release += slicetime;
    if(range_release>RANGE_MESURE)
    {
      short iFrontRange = get_range_sensor_gp2ya21(SENSOR_IR_FRONT);
      send_range(iFrontRange);
      range_release-=RANGE_MESURE;
    }
  }
}

